#include <compareFeatures.h>
#include <dirent.h>
#include <limits>
#include <fstream>

void compareCloth::init(const char* const ch, int in, int stateNum)
{

  sm.initProbabilityMap(stateNum);
  sm.initAliasVector(stateNum);
  sprintf(dirname, "%s",ch);
  compareNum = in;
  /* sprhTrain = loadSprhFeatures(); */
  /* histTrain = loadHistogram(); */
  /* partTrain = loadPartFeatures(); */

  sprhTrainVec = loadFeatureVec("sprhVec");
  histTrainVec = loadFeatureVec("histVec");
  partTrainVec = loadFeatureVec("partVec");



}

void compareCloth::setThreshold(float sprh, float hist, float part)
{
  sprhThre = sprh;
  histThre = hist;
  partThre = part;
}

void compareCloth::setThreVec(vector<float> inThreVec)
{
  for(uint i = 0; i < threVec.size(); i++)
    {
      try{
	threVec.at(i) = inThreVec.at(i);
      }
      catch (out_of_range& oor) {
	cerr << "Out of Range error: " << oor.what() << endl;
      }
    }
}

void compareCloth::getThreshold(float &sprh, float &hist, float &part)
{
  sprh = sprhThre;
  hist = histThre;
  part = partThre;
}

void compareCloth::getThreVec(vector<float> *outThreVec)
{
  for(uint i = 0; i < threVec.size(); i++)
    {
      outThreVec->push_back(threVec[i]);
    }
}

int compareCloth::convertStateNum(int num)
{
  return currentState * 1000 + num;
}

int compareCloth::calcStateNum(int num)
{
  return num / 1000;
}

int compareCloth::calcImageNum(int num)
{
  return num % 1000;
}

bool compareCloth::compareStateNum(int num)
{
  if(num == currentState || num >= compareNum)
    return true;
  else
    return false;
}

int compareCloth::getState(void)
{
  return currentState;
}

void compareCloth::setState(int num)
{
  currentState = num;
  sm.setCurrentState(num);
}

void compareCloth::showResult(  std::vector< vector< pair<int,float> > > result, cv::Mat srcimg, cv::Mat &output)
{
  float s;
  int i;
  sumResult(result, s, i);

  char filename[128];
  char imagecount[128];
  cv::Mat result_img;
  cv::Mat resize_img;
  cv::Mat database_img;

  resize(srcimg, resize_img, Size(), 0.5, 0.5, INTER_LINEAR);

  int width;
  int height;
  int i_ = i;
  i_ /= 1000;
  int check = checkState(i_);
  if(check != -1)
    cout << dirname << " " << "Match State " << check << endl;


  if(i == -1)
    {
      output = Mat::zeros( Size(srcimg.cols, srcimg.rows), CV_32F);
      width = output.cols + resize_img.cols;
      height = MAX(output.rows, resize_img.rows);
      vector<cv::Mat> images;
      vector<cv::Point> points;
      images.push_back(output);
      images.push_back(resize_img);
      points.push_back(Point(0.0));
      points.push_back(Point(output.cols+1,output.rows - resize_img.rows));
      combineImage(images, points, output);

      sprintf(imagecount, "NO MATCH IMAGE" );
      putText(output, imagecount , Point(srcimg.cols-20, 30), FONT_HERSHEY_SIMPLEX, 1.0, Scalar(255,255,255));
      sprintf(imagecount, "MATCH SCORE");
      putText(output, imagecount , Point(srcimg.cols-20, 80), FONT_HERSHEY_SIMPLEX, 1.0, Scalar(255,255,255));
      sprintf(imagecount, "%f", s);
      putText(output, imagecount , Point(srcimg.cols-20, 130), FONT_HERSHEY_SIMPLEX, 1.0, Scalar(255,255,255));
      sprintf(imagecount, "StateNum:%d", currentState);
      putText(output, imagecount , Point(srcimg.cols-20, 180), FONT_HERSHEY_SIMPLEX, 1.0, Scalar(255,255,255));


    }
  else
    {
      sprintf(filename, "%s/dst_img/flowimage_%05d.ppm", dirname, i);
      database_img = imread(filename);
      width = database_img.cols + resize_img.cols;
      height = MAX(database_img.rows, resize_img.rows);
      vector<cv::Mat> images;
      vector<cv::Point> points;
      images.push_back(database_img);
      images.push_back(resize_img);
      points.push_back(Point(0.0));
      points.push_back(Point(database_img.cols+1,database_img.rows - resize_img.rows));
      combineImage(images, points, output);
      sprintf(imagecount, "Match State:%d" ,i/1000);
      putText(output, imagecount , Point(database_img.cols-20, 30), FONT_HERSHEY_SIMPLEX, 1.0, Scalar(255,255,255));
      sprintf(imagecount, "MATCH SCORE");
      putText(output, imagecount , Point(database_img.cols-20, 80), FONT_HERSHEY_SIMPLEX, 1.0, Scalar(255,255,255));
      sprintf(imagecount, "%f", s);
      putText(output, imagecount , Point(database_img.cols-20, 130), FONT_HERSHEY_SIMPLEX, 1.0, Scalar(255,255,255));
      sprintf(imagecount, "StateNum:%d", currentState);
      putText(output, imagecount , Point(srcimg.cols-20, 180), FONT_HERSHEY_SIMPLEX, 1.0, Scalar(255,255,255));


    }
}
void compareCloth::showResultVec(vector< vector< pair<pair <int,int>,float> > > result, cv::Mat srcimg, cv::Mat &output)
{
  float s;
  pair<int,int> i;
  //sumVecResult(result, s, i);

  sumVecResult_withStateManipulator(result, s, i);

  char filename[128];
  char imagecount[128];
  cv::Mat resize_img;

  resize(srcimg, resize_img, Size(), 0.5, 0.5, INTER_LINEAR);
  int width;
  int height;

  // cout << "state " << i.first << endl;
  // cout << "imageNum " << i.second << endl;
  int check = checkState(i.first);
  setTempState(check);
  if(i.first == -1)
    {
      output = Mat::zeros( Size(srcimg.cols, srcimg.rows), CV_32F);

      if(dirname[0] == 'd')
	{
	  char tempfilename[128];
	  sprintf(tempfilename, "/home/jsk/oya/Pictures/si2011/output1/frame%04d.jpg",tempImageNum);
	  imwrite(tempfilename,output);
	  tempImageNum++;
	}

      width = output.cols + resize_img.cols;
      height = MAX(output.rows, resize_img.rows);
      vector<cv::Mat> images;
      vector<cv::Point> points;
      images.push_back(output);
      images.push_back(resize_img);
      points.push_back(Point(0.0));
      points.push_back(Point(output.cols+1,output.rows - resize_img.rows));
      combineImage(images, points, output);

      sprintf(imagecount, "NO MATCH IMAGE" );
      putText(output, imagecount , Point(srcimg.cols-20, 30), FONT_HERSHEY_SIMPLEX, 1.0, Scalar(255,255,255));
      sprintf(imagecount, "MATCH SCORE");
      putText(output, imagecount , Point(srcimg.cols-20, 80), FONT_HERSHEY_SIMPLEX, 1.0, Scalar(255,255,255));
      sprintf(imagecount, "%f", s);
      putText(output, imagecount , Point(srcimg.cols-20, 130), FONT_HERSHEY_SIMPLEX, 1.0, Scalar(255,255,255));
      sprintf(imagecount, "StateNum:%d", currentState);
      putText(output, imagecount , Point(srcimg.cols-20, 180), FONT_HERSHEY_SIMPLEX, 1.0, Scalar(255,255,255));




    }
  else
    {
      sprintf(filename, "%s/src_image/src_image_%04d_%07d.ppm", dirname, i.first, i.second);
      resultMatchImg = imread(filename);
      width = resultMatchImg.cols + resize_img.cols;
      height = MAX(resultMatchImg.rows, resize_img.rows);
      vector<cv::Mat> images;
      vector<cv::Point> points;
      images.push_back(resultMatchImg);
      images.push_back(resize_img);
      points.push_back(Point(0.0));
      points.push_back(Point(resultMatchImg.cols+1,resultMatchImg.rows - resize_img.rows));
      combineImage(images, points, output);
      sprintf(imagecount, "Match State:%d" ,i.first);
      putText(output, imagecount , Point(resultMatchImg.cols-20, 30), FONT_HERSHEY_SIMPLEX, 1.0, Scalar(255,255,255));
      sprintf(imagecount, "MATCH SCORE");
      putText(output, imagecount , Point(resultMatchImg.cols-20, 80), FONT_HERSHEY_SIMPLEX, 1.0, Scalar(255,255,255));
      sprintf(imagecount, "%f", s);
      putText(output, imagecount , Point(resultMatchImg.cols-20, 130), FONT_HERSHEY_SIMPLEX, 1.0, Scalar(255,255,255));
      sprintf(imagecount, "StateNum:%d", currentState);
      putText(output, imagecount , Point(srcimg.cols-20, 180), FONT_HERSHEY_SIMPLEX, 1.0, Scalar(255,255,255));

      if(dirname[0] == 'd')
	{
	  char tempfilename[128];
	  sprintf(tempfilename, "/home/jsk/oya/Pictures/si2011/output1/frame%04d_st%04d_num%07d.jpg",tempImageNum,i.first,i.second);
	  imwrite(tempfilename,resultMatchImg);
	  tempImageNum++;
	}

      //showDatabaseFlow(i.first, i.second);

    }

}


void compareCloth::stateMatch(int imnum, std::vector<Point2f> flowPoint, std::vector<Point2f> flowScale, cv::Mat input)
{
  std::vector< pair<P2d,P2d> > vals;
  std::vector< pair<int, float> >sprhResult, histResult, partResult, meanResult;
  std::vector< vector< pair<int,float> > > result;
  convertflowPoint2P2d(flowPoint, flowScale, &vals);
  compareSprh(imnum, flowPoint, flowScale, sprhTrain, &sprhResult);
  compareHist(imnum, flowPoint, flowScale, input, histTrain, &histResult);
  comparePart(imnum, vals, input, partTrain, &partResult);
  //compareMean(imnum, vals, input, partTrain, &partResult);
  result.push_back(sprhResult);
  result.push_back(histResult);
  result.push_back(partResult);

  displayResult2(sprhResult, "sprh", sprhThre, input, imnum);
  displayResult2(histResult, "hist", histThre, input, imnum);
  displayResult2(partResult, "part", partThre, input, imnum);


  // int i = sumResult(result, input);
  // char filename[128];
  // char imagecount[128];
  // cv::Mat result_img;

  // if(i == -1)
  //   {
  //     result_img = Mat::zeros(input.rows, input.cols, CV_32F);
  //   }
  // else
  //   {
  //     sprintf(filename, "data/dst_img/flowimage_%05d.ppm",i);
  //     result_img = imread(filename);
  //     sprintf(imagecount, "image%05d.ppm", i);
  //     putText(result_img, imagecount, Point(15,30), FONT_HERSHEY_SIMPLEX, 1.0, Scalar(255,255,255));
  //   }
  cv::Mat result_img;

  showResult(result, input, result_img);

  char matchResult[128];
  sprintf(matchResult, "matchResult:%s", dirname);
  imshow(matchResult, result_img);
  char fn[128];
  sprintf(fn, "movie/7/matchResult/image%05d.ppm", imnum);

  imwrite(fn, result_img);

}

void compareCloth::stateVecMatch(int imageNum, std::vector<Point2f> flowPoint, std::vector<Point2f> flowScale, cv::Mat input, cv::Mat &output)
{
  std::vector< pair< pair<int,int> , float > > sprhResult, histResult, partResult;
  std::vector< std::vector< pair< pair<int, int> , float > > > result;
  vector<float> sprhFeature, histFeature, partFeature;


  genSprhVec(flowPoint, flowScale, &sprhFeature);
  genHistVec(flowPoint, flowScale, input, &histFeature);
  genPartVec(flowPoint, flowScale, input, &partFeature);


  compareFeatureVec( sprhFeature, sprhTrainVec, &sprhResult, CV_COMP_MANHATTAN);
  compareFeatureVec( histFeature, histTrainVec, &histResult, CV_COMP_BHATTACHARYYA);
  compareFeatureVec( partFeature, partTrainVec, &partResult, CV_COMP_MANHATTAN);



  result.push_back(sprhResult);
  result.push_back(histResult);
  result.push_back(partResult);

  displayResultVec(sprhResult, "sprh", threVec[0], input, imageNum);
  displayResultVec(histResult, "hist", threVec[1], input, imageNum);
  displayResultVec(partResult, "part", threVec[2], input, imageNum);



  // for(uint i = 0; i < sprhResult.size(); i++)
  //   if(sprhResult[i].second >10 || sprhResult[i].second <=0)
  //     {

  // 	{
  // 	  //cout << "sprhResult[i].first "<< sprhResult[i].first << endl;
  // 	  cout << "sprhResult[i].first.first "<< sprhResult[i].first.first << endl;
  // 	  cout << "sprhResult[i].first.second "<< sprhResult[i].first.second << endl;
  // 	  cout << "sprhResult[i].second "<< sprhResult[i].second << endl;
  // 	}
  //     }
  // for(uint i = 0; i < histResult.size(); i++)
  //   if(histResult[i].second >10 || histResult[i].second <=0)
  //     {

  // 	{
  // 	  //cout << "sprhResult[i].first "<< sprhResult[i].first << endl;
  // 	  cout << "histResult[i].first.first "<< histResult[i].first.first << endl;
  // 	  cout << "histResult[i].first.second "<< histResult[i].first.second << endl;
  // 	  cout << "histResult[i].second "<< histResult[i].second << endl;
  // 	}
  //     }
  // for(uint i = 0; i < partResult.size(); i++)
  //   {
  //     if(partResult[i].second >10 || partResult[i].second <=0)
  // 	{

  // 	  //cout << "partResult[i].first "<< partResult[i].first << endl;
  // 	  cout << "partResult[i].first.first "<< partResult[i].first.first << endl;
  // 	  cout << "partResult[i].first.second "<< partResult[i].first.second << endl;
  // 	  cout << "partResult[i].second "<< partResult[i].second << endl;
  // 	}
  //   }


  showResultVec(result, input, output);
  char matchResult[128];
  sprintf(matchResult, "matchResult:%s", dirname);
  imshow(matchResult, output);
  char fn[128];
  sprintf(fn, "movie/2/%s/image%05d.ppm", dirname, imageNum);
  imwrite(fn, output);


}


float compareCloth::calcFeatureDist(vector<float> currentVec, vector<float> learnedVec, int method)
{
  if(currentVec.size() != learnedVec.size())
    {
      std::cerr << "current vector size is different from learned vectors" << std::endl;
    }
  float dist = 0;

  if(method == CV_COMP_MANHATTAN)
    {
     for(uint i = 0; i < currentVec.size(); i++)
	{

	  dist += fabs(currentVec[i] - learnedVec[i]);
	}
     return dist/currentVec.size();
    }
  else
    {
      MatND first(currentVec);
      MatND second(learnedVec);
      dist = (float)cv::compareHist(first,second,CV_COMP_BHATTACHARYYA);
      return dist;
    }
}

void compareCloth::compareFeatureVec(vector<float> currentVec, std::vector<featureVec> trainVec, std::vector<pair<pair<int,int>, float> > *result, int method)
{ 
  float dist = std::numeric_limits<float>::max();
  int index = -1;
  for(uint i = 0; i < trainVec.size(); i++)
    {
      featureVec *fv = &(trainVec.at(i));
      float dif=0.0;
      dif = calcFeatureDist(currentVec, fv->features, method);
      if(compareStateNum(fv->stateNum))
	{
	  if(dist > dif)
	    {
	      dist = dif;
	    }
	  pair<pair<int,int>, float> res;
	  res.first.first = fv->stateNum;
	  res.first.second = fv->imageNum;
	  res.second = dif;
	  result->push_back(res);
	}
    }
}

void compareCloth::compareSprh(int imnum,const std::vector<Point2f> flowPoint, const std::vector<Point2f> flowScale, std::vector<SPRHfeature> sprh_train ,std::vector<pair<int, float> > *result)
{
  if(flowPoint.size() != flowScale.size())
    {
      std::cerr << "flowPoint is different from flowScale" << std::endl;
      exit(1);
    }

  vector< pair<P2d,P2d> > flinfo;
  for( uint i = 0; i < flowPoint.size(); i++)
    {
      P2d first = pair<float,float>(flowScale[i].x, flowScale[i].y);
      P2d second = pair<float,float>(flowPoint[i].x, flowPoint[i].y);
      flinfo.push_back(pair<P2d,P2d>(first,second));
    }
  SPRH2d sprh(5,500);
  sprh.proc(flinfo);
  int hn;
  float *hist = sprh.getHistogram(&hn);

  float dist = std::numeric_limits<float>::max();
  int index = -1;

  for(uint n=0; n<sprh_train.size(); n++)
    {
      SPRHfeature *sf = &(sprh_train.at(n));
      float dif=0.0;
      for(int i=0; i<hn; i++)
        dif += fabs(sf->val[i] - hist[i]);

      int statenum = sf->sn/1000;
      if(compareStateNum(statenum))
	{
	  if(dist > dif)
	    {
	      dist = dif;
	      index = sf->sn;
	    }
	  pair<int, float> res;
	  res.first = sf->sn;
	  res.second = dif;
	  result->push_back(res);
	}
      // if(dif < 0.001)
      //   printf("perhaps, imnum %d and sprh_train %d are same number?\n",imnum,sf->sn);
    }
  // if(index > 0)
  //   {
  //     result->first = index;
  //     result->second = dist;
  //   }
}

void compareCloth::compareHist(int imn, std::vector<Point2f> flowPoint, std::vector<Point2f> flowScale, cv::Mat input, std::vector<histFeatures> hlist, std::vector<pair<int, float> > *result)
{
  cv::Mat input_img = genFlowHist(flowScale, flowPoint, input);

  IplImage temp_img = input_img;
  IplImage *hist_img = &temp_img;

  int hist_size = 256;
  float range_0[] = { 0, 256 };
  float *ranges[] = { range_0 };
  CvHistogram *hist;
  hist = cvCreateHist (1, &hist_size, CV_HIST_ARRAY, ranges, 1);
  cvCalcHist ( &hist_img, hist, 0 , NULL);
  //黒い部分を無視
  cvSet1D(hist->bins,0,cvScalar(0));

  cvNormalizeHist(hist, 10000);

  float dist = std::numeric_limits<float>::max();
  float temp;
  int index = -1;

  for(uint i = 0; i<hlist.size(); i++)
    {
      histFeatures *hist_temp = &(hlist.at(i));
      temp = cvCompareHist(hist, hist_temp->hist, CV_COMP_BHATTACHARYYA);
      //printf("datanum %d  hist %f \n",hist_temp->hn , temp); 
      int statenum = hist_temp->hn/1000;
      if(compareStateNum(statenum))
	{
	  if(dist > temp){
	    dist = temp;
	    index = hist_temp->hn;
	  }
	  pair<int, float> res;
	  res.first = hist_temp->hn;
	  res.second = temp;
	  result->push_back(res);
	}

    }
  // if(index > 0)
  //   {
  //     result->first = index;
  //     result->second = dist;
  //   }

}

void compareCloth::comparePart(int imn, vector< pair<P2d,P2d> > vals, cv::Mat input, std::vector<partFeatures> part_train, std::vector<pair<int, float> > *result)
{
  float val1[3];
  pair<P2d,P2d> mean1,var1;
  calcMeanVar(vals, &mean1, &var1);
  Point2f cent;
  calcImageCenter(input, &cent);

  //二点間のベクトル
  val1[0] = mean1.first.first - cent.x;
  val1[1] = mean1.first.second - cent.y;
  val1[0] /= (float)input.cols;
  val1[1] /= (float)input.rows;

  //布領域とフロー領域の比率
  val1[2] =  36 * sqrt(var1.first.first) * sqrt(var1.first.second)  / (float)countPic(input);



  float dist = std::numeric_limits<float>::max();
  int index = -1;

  for(uint n=0; n<part_train.size(); n++)
    {
      float val2[3];
      partFeatures *pf = &(part_train.at(n));
      vector< pair<P2d,P2d> > temp;

      for(int i = 0; i< pf->fnum; i++)
        {
          P2d a = pair<float,float>((float)pf->x[i],(float)pf->y[i]);
          P2d b = pair<float,float>(pf->len[i],pf->ang[i]);
          temp.push_back(pair<P2d,P2d>(a,b));
        }
      pair<P2d,P2d> mean2, var2;
      calcMeanVar(temp, &mean2, &var2);
      calcImageCenter(pf->img, &cent);

      //二点間のベクトル
      val2[0] = mean2.first.first - cent.x;
      val2[1] = mean2.first.second - cent.y;
      val2[0] /= (float)input.cols;
      val2[1] /= (float)input.rows;

      //布領域とフロー領域の比率
      val2[2] =  36 * sqrt(var2.first.first) * sqrt(var2.first.second)  / (float)countPic(input);


      float dif=0.0;
      for(int i=0; i<3; i++)
	{
	  //cout << "val1 " << val1[i] << " val2 " << val2[i] << endl;
	  dif += fabs(val1[i] - val2[i]);
	}
      int statenum = pf->num/1000;
      if(compareStateNum(statenum))
	{
	  if(dist > dif){
	    dist = dif;
	    index = pf->num;
	  }
	  pair<int, float> res;
	  res.first = pf->num;
	  res.second = dif;
	  result->push_back(res);
	}


      //printf("datanum %d part dif = %f\n",pf->num,dif);
      // if(dif < 0.001)
      //   printf("perhaps, imn %d and part_train %d are same number?\n",imn,pf->num);

    }

}

void compareCloth::compareMean(int imn, vector< pair<P2d,P2d> > vals, cv::Mat input, std::vector<partFeatures> part_train, std::vector<pair<int, float> > *result)
{
  float val1[4];
  pair<P2d,P2d> mean1,var1;
  calcMeanVar(vals, &mean1, &var1);
  val1[0] = mean1.second.first;
  val1[1] = mean1.second.second;
  val1[2] = var1.second.first;
  val1[3] = var1.second.second;

  float dist = std::numeric_limits<float>::max();
  int index = -1;

  for(uint n=0; n<part_train.size(); n++)
    {
      float val2[2];
      partFeatures *pf = &(part_train.at(n));
      vector< pair<P2d,P2d> > temp;

      for(int i = 0; i< pf->fnum; i++)
        {
          P2d a = pair<float,float>((float)pf->x[i],(float)pf->y[i]);
          P2d b = pair<float,float>(pf->len[i],pf->ang[i]);
          temp.push_back(pair<P2d,P2d>(a,b));
        }
      pair<P2d,P2d> mean2, var2;
      calcMeanVar(temp, &mean2, &var2);
      val2[0] = mean2.second.first;
      val2[1] = mean2.second.second;
      val2[2] = var2.second.first;
      val2[3] = var2.second.second;


      float dif=0.0;

      dif += fabs(val1[0] - val2[0])/3;
      dif += fabs(val1[1] - val2[1])/1.5;
      dif += fabs(val1[2] - val2[2])/3;
      dif += fabs(val1[3] - val2[3])/1.5;
      if ( pf->num == 3)
	for( int i = 0; i<4; i++)
	  cout << "cout  "<< pf->num <<" " << i << " " << fabs(val1[i] - val2[i]) << endl;


      int statenum = pf->num/1000;
      if(compareStateNum(statenum))
	{
	  if(dist > dif){
	    dist = dif;
	    index = pf->num;
	  }
	  pair<int, float> res;
	  res.first = pf->num;
	  res.second = dif;
	  result->push_back(res);
	}


      //printf("datanum %d part dif = %f\n",pf->num,dif);
      // if(dif < 0.001)
      //   printf("perhaps, imn %d and part_train %d are same number?\n",imn,pf->num);

    }


}

void compareCloth::genfiles(int imnum, std::vector<Point2f> flowPoint, std::vector<Point2f> flowScale, cv::Mat input)
{

  vector< pair<P2d,P2d> > vals, selected;
  convertflowPoint2P2d(flowPoint, flowScale, &vals);
  float param[2][2];

  //stateの数字をつけたし
  int temp = imnum;
  imnum = convertStateNum(temp);


  simpleRansac(vals,param);
  selectPoints(vals, param, &selected);

  genImageFile(imnum, input);
  genHistFile(imnum, flowScale, flowPoint, input);
  genSprhFile(imnum, flowPoint, flowScale);
  genPartFile(imnum, vals, selected, input);
}

void compareCloth::genVecFiles(std::vector<Point2f> flowPoint, std::vector<Point2f> flowScale, cv::Mat input)
{


  genImageFileVec(input);
  vector<float> sprhVec,histVec,partVec;

  genSprhVec(flowPoint, flowScale, &sprhVec);
  genHistVec (flowPoint, flowScale, input, &histVec);
  genPartVec(flowPoint, flowScale, input, &partVec);
  //save flows
  char filename[128];
  sprintf(filename, "%s/sprhVec/sprhVec_%04d_%07d.dat", dirname, currentState,  eachImageNum);
  saveFeatureVec(filename, sprhVec);

  sprintf(filename, "%s/histVec/histVec_%04d_%07d.dat", dirname, currentState,  eachImageNum);
  saveFeatureVec(filename, histVec);

  sprintf(filename, "%s/partVec/partVec_%04d_%07d.dat", dirname, currentState, eachImageNum);
  saveFeatureVec(filename, partVec);

  eachImageNum++;
}

void compareCloth::genImageFile(int imnum, cv::Mat input)
{
  char filename[128];
  sprintf(filename,"%s/dst_img/flowimage_%05d.ppm", dirname, imnum);
  imwrite(filename,input);
}

void compareCloth::genImageFileVec(cv::Mat input)
{
  char filename[128];
  sprintf(filename,"%s/src_image/src_image_%04d_%07d.ppm", dirname, currentState, eachImageNum);
  imwrite(filename,input);
}


void compareCloth::genSprhFile(int imnum, const std::vector<Point2f> flowPoint, const std::vector<Point2f> flowScale)
{
  if(flowPoint.size() != flowScale.size())
    {
      std::cerr << "flowPoint is different from flowScale" << std::endl;
      exit(1);
    }

  vector< pair<P2d,P2d> > flinfo;
  for( uint i = 0; i < flowPoint.size(); i++)
    {
      P2d first = pair<float,float>(flowScale[i].x, flowScale[i].y);
      P2d second = pair<float,float>(flowPoint[i].x, flowPoint[i].y);
      flinfo.push_back(pair<P2d,P2d>(first,second));
    }

  SPRH2d sprh(5,500);

  sprh.proc(flinfo);

  char filename[128];
  sprintf(filename, "%s/sprh/sprh2d_%05d.dat", dirname, imnum);
  FILE *fp = fopen(filename,"w");
  int hn=0;
  float *hist = sprh.getHistogram(&hn);
  for(int i=0; i<hn; i++)
    fprintf(fp,"%f\n",hist[i]);
  fclose(fp);

}

void compareCloth::genHistFile(int imnum, std::vector<Point2f> flowScale, std::vector<Point2f> flowPoint, cv::Mat input)
{
  if(flowPoint.size() != flowScale.size())
    {
      std::cerr << "flowPoint is different from flowScale" << std::endl;
      exit(1);
    }

  cv::Mat hist_img = genFlowHist(flowScale, flowPoint, input);

  char filename[128];
  sprintf(filename, "%s/hist_img/frame%05d.ppm", dirname, imnum);
  imwrite(filename, hist_img);
}

int compareCloth::genPartFile(int imnum, const std::vector< pair<P2d,P2d> > vals, const std::vector< pair<P2d,P2d> > selected, cv::Mat input)
{
  if(selected.size() < 10){
    printf("selected not enough flows observed\n");
    return 0;
  }

  char filename[128];

  sprintf(filename,"%s/all_flow/allflow_%05d.dat", dirname, imnum);
  FILE *fp = fopen(filename,"w");
  vector< pair<P2d,P2d> >::const_iterator it = vals.begin();
  for(; it != vals.end(); it++)
    fprintf(fp,"%f %f %f %f\n",it->first.first, it->first.second, it->second.first,it->second.second);

  fclose(fp);

  sprintf(filename,"%s/selected_flow/selectedflow_%05d.dat", dirname, imnum);
  fp = fopen(filename,"w");
  it = selected.begin();
  for(; it != selected.end(); it++)
    fprintf(fp,"%f %f %f %f\n",it->first.first, it->first.second, it->second.first,it->second.second);
  fclose(fp);

  return 0;
}

int compareCloth::genAllFlowFile(const std::vector<Point2f> flowPoint, const std::vector<Point2f> flowScale)
{
  if(flowPoint.size() != flowScale.size())
    {
      std::cerr << "flowPoint is different from flowScale" << std::endl;
      exit(1);
    }
  char filename[128];
  sprintf(filename,"%s/allFlowRect/allR_%04d_%07d.dat",dirname, currentState, eachImageNum);
  ofstream ofsRect(filename);
  for(uint i = 0; i<flowPoint.size(); i++)
    {
      ofsRect << flowPoint[i].x << " "
	      << flowPoint[i].y << " "
	      << flowScale[i].x << " "
	      << flowScale[i].y << endl;
    }
  ofsRect.close();

  sprintf(filename,"%s/allFlowPolar/allP_%04d_%07d.dat",dirname, currentState, eachImageNum);
  ofstream ofsPolar(filename);
  vector< pair<P2d,P2d> > vals;
  convertflowPoint2P2d(flowPoint, flowScale, &vals);
  for(uint i = 0; i<vals.size(); i++)
    {
      ofsPolar << vals[i].first.first << " "
	       << vals[i].first.second << " "
	       << vals[i].second.first << " "
	       << vals[i].second.second << endl;
    }
  ofsPolar.close();


}

void compareCloth::genSprhVec(const std::vector<Point2f> flowPoint, const std::vector<Point2f> flowScale, vector<float> *sprhVec)
{
  if(flowPoint.size() != flowScale.size())
    {
      std::cerr << "flowPoint is different from flowScale" << std::endl;
      exit(1);
    }

  vector< pair<P2d,P2d> > flinfo;
  for( uint i = 0; i < flowPoint.size(); i++)
    {
      P2d first = pair<float,float>(flowScale[i].x, flowScale[i].y);
      P2d second = pair<float,float>(flowPoint[i].x, flowPoint[i].y);
      flinfo.push_back(pair<P2d,P2d>(first,second));
    }

  SPRH2d sprh(5,500);

  sprh.proc(flinfo);

  char filename[128];
  sprintf(filename, "%s/sprhVec/sprh%04d_%07d.dat", dirname, currentState,  eachImageNum);
  sprh.getHistVec(sprhVec);

}

void compareCloth::genHistVec(const std::vector<Point2f> flowPoint, const std::vector<Point2f> flowScale, cv::Mat input, vector<float> *histVec)
{
  if(flowPoint.size() != flowScale.size())
    {
      std::cerr << "flowPoint is different from flowScale" << std::endl;
      exit(1);
    }
  
  IplImage temp;
  IplImage *im;
  CvHistogram *hist;
  cv::Mat hist_img = genFlowHist(flowScale, flowPoint, input);
  temp = hist_img;
  im = &temp;
  int hist_size = 256;
  float range_0[] = { 0, 256 };
  float *ranges[] = { range_0 };

  int imageNum = 0;



  hist = cvCreateHist (1, &hist_size, CV_HIST_ARRAY, ranges, 1);

  cvCalcHist ( &im, hist, 0, NULL);
  cvSet1D(hist->bins,0,cvScalar(0));
  cvNormalizeHist(hist, 10000);

  for(int i = 0; i < hist_size; i++)
    {
      histVec->push_back(cvQueryHistValue_1D( hist, i));
    }

}


void compareCloth::genPartVec(const std::vector<Point2f> flowPoint, const std::vector<Point2f> flowScale, cv::Mat input, vector<float> *partVec)
{

  vector< pair<P2d,P2d> > vals, selected;
  convertflowPoint2P2d(flowPoint, flowScale, &vals);
  float param[2][2];
  //cout << "vals count " << vals.size() << endl;
  simpleRansac(vals,param);
  selectPoints(vals, param, &selected);
  float val[3];
  pair<P2d, P2d> mean,var;
  calcMeanVar(vals, &mean, &var);
  Point2f center;
  calcImageCenter(input, &center);

  //2点間のベクトル
  val[0] = mean.first.first - center.x;
  val[1] = mean.first.second - center.y;
  val[0] /= (float)input.cols;
  val[1] /= (float)input.rows;

  //布領域とフロー領域の比率
  val[2] = 2*3 * 2*3 * sqrt(var.first.first) * sqrt(var.first.second) / (float)countPic(input);


  for(int i = 0; i < 3; i++)
    {
      partVec->push_back(val[i]);
    }

}

void compareCloth::saveFeatureVec(char *filename, vector<float> features)
{
  FILE *fp = fopen(filename, "w");
  for(int i = 0; i < features.size(); i++)
    fprintf(fp, "%f\n",features[i]);
  fclose(fp);
}

std::vector<SPRHfeature> compareCloth::loadSprhFeatures(void)
{
  std::vector<SPRHfeature> flist;

  DIR *dir_ptr;
  struct dirent *ent;
  char sprhdir[128];
  sprintf(sprhdir, "%s/sprh/", dirname);
  cout << "dir name " << sprhdir << endl;
  if((dir_ptr = opendir(sprhdir)) == (DIR *)NULL)
    {
      std::cerr << "no sprh directory" << std::endl;
      exit(1);
    }

  //指定したディレクトリ内部には特徴ファイル以外は存在しないことを想定
  FILE *fp;
  int featcount=0,imnum=0;



  for(int i=0; (ent = readdir(dir_ptr)) != (struct dirent *)NULL; i++){

    if(ent->d_name[0] != '.' && sscanf(ent->d_name,"sprh2d_%05d.dat",&imnum) !=
0)
      {
        float val;
	if(featcount == 0) //最初だけ、特徴点数を数えるために二回呼ぶ
          {
            string opf = string(sprhdir) + string(ent->d_name);
            fp = fopen(opf.c_str(),"r");
            while(fscanf(fp,"%f\n",&val) != EOF)
              featcount++;
            fclose(fp);
          }
	SPRHfeature sf;
        sf.sn = imnum;
        sf.val = new float[featcount];

	string opf = string(sprhdir) + string(ent->d_name);
        fp = fopen(opf.c_str(),"r");

	int n = 0;
        while(fscanf(fp,"%f\n",&val) != EOF)
          {
            sf.val[n] = val;
            n++;
          }
        fclose(fp);
        flist.push_back(sf);

	featcount = 0;
      }
  }

  closedir(dir_ptr);

  return flist;
}

std::vector<histFeatures> compareCloth::loadHistogram(void)
{
  std::vector<histFeatures> hlist;
  histFeatures hist;
  IplImage *temp;
  char tempfile[128];
  int hist_size = 256;
  float range_0[] = { 0, 256 };
  float *ranges[] = { range_0 };
  DIR *dir_ptr;
  struct dirent *ent;
  char histdir[128];
  sprintf(histdir, "%s/hist_img/", dirname);
  cout << "dir name " << histdir << endl;
  if((dir_ptr = opendir(histdir)) == (DIR *)NULL)
    {
      std::cerr << "no hist_img directory" << std::endl;
      exit(1);
    }
  //指定したディレクトリ内部には特徴ファイル以外は存在しないことを想定          

  int imnum=0;
  for(int i=0; (ent = readdir(dir_ptr)) != (struct dirent *)NULL; i++)
    if(sscanf(ent->d_name,"frame%05d.ppm",&imnum) != 0)
      {
        sprintf(tempfile, "%s%s",histdir, ent->d_name);
        temp = cvLoadImage(tempfile , CV_LOAD_IMAGE_GRAYSCALE);
	hist.hist = cvCreateHist (1, &hist_size, CV_HIST_ARRAY, ranges, 1);
        cvCalcHist ( &temp, hist.hist, 0 , NULL);
        //黒い部分を無視
	cvSet1D(hist.hist->bins,0,cvScalar(0));

        cvNormalizeHist(hist.hist, 10000);
        hist.hn = imnum;
        hlist.push_back(hist);
      }
  closedir(dir_ptr);

  return hlist;
}

vector<partFeatures>compareCloth::loadPartFeatures(void)
{
  std::vector<partFeatures> flist;

  DIR *dir_ptr;
  struct dirent *ent;
  char flowdir[128];
  sprintf(flowdir,"%s/selected_flow/", dirname);
  cout << "dir name " << flowdir << endl;
  if((dir_ptr = opendir(flowdir)) == (DIR *)NULL)
    {
      std::cerr << "no selected_flow directory" << std::endl;
      exit(1);
    }

  //指定したディレクトリ内部には特徴ファイル以外は存在しないことを想定
  FILE *fp;


  int featcount=0,imnum=0;
  for(int i=0; (ent = readdir(dir_ptr)) != (struct dirent *)NULL; i++){
    if(ent->d_name[0] != '.' && sscanf(ent->d_name,"selectedflow_%05d.dat",&imnum) != 0)
      {
        float x;
        float y;
        float len;
        float ang;
        if(featcount == 0);
        {
          string opf = string(flowdir) + string(ent->d_name);
          fp = fopen(opf.c_str(),"r");
          while(fscanf(fp,"%f %f %f %f\n",&x, &y, &len, &ang) != EOF)
            featcount++;
          fclose(fp);
        }

        partFeatures pf;
        pf.num = imnum;
        pf.fnum = featcount;

	pf.x = new float[featcount];
        pf.y = new float[featcount];
        pf.len = new float[featcount];
        pf.ang = new float[featcount];
        char file[128];
        sprintf(file, "%s/dst_img/flowimage_%05d.ppm", dirname, pf.num);
        pf.img = imread( file);
        string opf = string(dirname) + string("/selected_flow/") + string(ent->d_name);
        if((fp = fopen(opf.c_str(),"r")) == NULL)
          {
            cerr << "no selected_flow file" << endl;
            printf("%s\n",opf.c_str());
            exit(1);
          }
        int n = 0;
        while(fscanf(fp,"%f %f %f %f\n",&x, &y, &len, &ang) != EOF)
          {
            pf.x[n] = x;
            pf.y[n] = y;
            pf.len[n] = len;
            pf.ang[n] = ang;
            n++;
          }
        fclose(fp);
        flist.push_back(pf);

      }
  }
  closedir(dir_ptr);
  return flist;  

}

std::vector<featureVec> compareCloth::loadFeatureVec(char *featuredir)
{
  std::vector<featureVec> flist;

  DIR *dir_ptr;
  struct dirent *ent;
  char filedir[128];
  sprintf(filedir, "%s/%s/", dirname, featuredir);
  cout << "dirname: " << dirname << endl;
  cout << "file dir: " << filedir << endl;
  if((dir_ptr = opendir(filedir)) == (DIR *)NULL)
    {
      std::cerr << "no " << filedir << "directory " << std::endl;
      exit(1);
    }

  //指定したディレクトリ内部には特徴ファイル以外は存在しないことを想定
  FILE *fp;
  int statenum=0,imnum=0;

  char scanfile[128];
  sprintf(scanfile, "%s_%%04d_%%07d.dat", featuredir);


  for(int i=0; (ent = readdir(dir_ptr)) != (struct dirent *)NULL; i++){

    if(ent->d_name[0] != '.' && sscanf(ent->d_name,scanfile,&statenum, &imnum) != 0)
      {
	featureVec fv;
	float val;
        fv.imageNum = imnum;
        fv.stateNum = statenum;

	string opf = string(filedir) + string(ent->d_name);
        fp = fopen(opf.c_str(),"r");

        while(fscanf(fp,"%f\n",&val) != EOF)
          {
            fv.features.push_back(val);
          }
        fclose(fp);
        flist.push_back(fv);
      }
  }
  closedir(dir_ptr);
  return flist;
}


int compareCloth::displayResult(std::vector<pair<int, float> > result, char *windowname, float shre, cv::Mat input)
{
  int maxindex = -1;
  float maxdif = std::numeric_limits<float>::max();
  for(uint i = 0; i < result.size(); i++)
    {
      if(result[i].second < shre && result[i].second < maxdif)
	{
	  maxindex = result[i].first;
	  maxdif = result[i].second;
	}
    }

  char filename[128];
  char imagecount[128];
  cv::Mat result_img;

  if(maxindex == -1)
    {
      result_img = Mat::zeros(input.rows, input.cols, CV_32F);
    }
  else
    {
      sprintf(filename, "%s/dst_img/flowimage_%05d.ppm", dirname,maxindex);
      result_img = imread(filename);
      sprintf(imagecount, "image%05d.ppm", maxindex);
      putText(result_img, imagecount, Point(15,30), FONT_HERSHEY_SIMPLEX, 1.0, Scalar(255,255,255));
    }

  char winname[128];
  sprintf(winname,"%s:%s", windowname, dirname);
  imshow(winname, result_img);
  return 0;
}

int compareCloth::displayResult2(std::vector<pair<int, float> > result, char *windowname, float shre, cv::Mat input, int imnum)
{
  int maxindex = -1;
  float maxdif = std::numeric_limits<float>::max();
  for(uint i = 0; i < result.size(); i++)
    {
      if(result[i].second < shre && result[i].second < maxdif)
	{
	  maxindex = result[i].first;
	  maxdif = result[i].second;
	}
    }

  char filename[128];
  char imagecount[128];
  cv::Mat result_img;

  if(maxindex == -1)
    {
      result_img = Mat::zeros(input.rows, input.cols, CV_32F);
    }
  else
    {
      sprintf(filename, "%s/dst_img/flowimage_%05d.ppm", dirname, maxindex);
      result_img = imread(filename);
      sprintf(imagecount, "image%05d.ppm", maxindex);
      putText(result_img, imagecount, Point(15,30), FONT_HERSHEY_SIMPLEX, 1.0, Scalar(255,255,255));
    }
  char winname[128];
  sprintf(winname,"%s:%s", windowname, dirname);
  imshow(winname, result_img);
  char fn[128];
  sprintf(fn, "movie/7/%s/frame%05d.ppm",windowname, imnum);
  imwrite(fn, result_img);

  return 0;
}

int compareCloth::displayResultVec(std::vector<pair< pair<int,int>, float> > result, char *windowname, float shre, cv::Mat input, int imnum)
{
  pair<int,int> maxindex(-1,-1);
  //maxindex = (-1,-1);
  float maxdif = std::numeric_limits<float>::max();
  for(uint i = 0; i < result.size(); i++)
    {
      if(result[i].second < shre && result[i].second < maxdif)
	{
	  maxindex = result[i].first;
	  maxdif = result[i].second;
	}
    }

  char filename[128];
  char imagecount[128];
  cv::Mat result_img;
  if(maxindex.first == -1)
    {
      result_img = Mat::zeros(input.rows, input.cols, CV_32F);
    }
  else
    {
      sprintf(filename, "%s/src_image/src_image_%04d_%07d.ppm", dirname, maxindex.first, maxindex.second);
      result_img = imread(filename);
      sprintf(imagecount, "image%04d_%07d.ppm", maxindex.first, maxindex.second);
      putText(result_img, imagecount, Point(15,30), FONT_HERSHEY_SIMPLEX, 1.0, Scalar(255,255,255));
    }

  char winname[128];
  sprintf(winname,"%s:%s", windowname, dirname);

  imshow(winname, result_img);
  char fn[128];
  // sprintf(fn, "movie/7/%s/frame%05d.ppm",windowname, imnum);
  // imwrite(fn, result_img);
  return 0;
}


void compareCloth::sumResult(vector< vector< pair<int,float> > > result, float &maxSum, int &maxIndex)
{
  int flag[3];
  maxSum = std::numeric_limits<float>::max();
  float maxsum = std::numeric_limits<float>::max();
  float maxsum_diff = std::numeric_limits<float>::max();
  int maxindex = -1;
  for(uint i = 0; i < result[0].size(); i++)
    {
      for(int k = 0; k<3 ; k++)
	flag[k] = 0;
      float sum = result[0][i].second/sprhThre;
      if(result[0][i].second < sprhThre)
	{
	  flag[0] = 1;
	}
      else
	{
	  flag[0] = 0;
	}

      for(uint j = 0; j < result[0].size(); j++)
	{
	  if(result[0][i].first == result[1][j].first)
	    {
	      sum *= result[1][j].second/histThre;
	      if( result[1][j].second < histThre)
		{
		  flag[1] = 1;
		}
	      else
		{
		  flag[1] = 0;
		}
	    }
	  if(result[0][i].first == result[2][j].first)
	    {
	      sum *= result[2][j].second/partThre;
	      if(result[2][j].second < partThre)
		{
		  flag[2] = 1;
		}
	      else
		{
		  flag[2] = 0;
		}
	    }
	}


      if(flag[0] == 1 && flag[1] == 1 && flag[2] == 1)
	{
	  if(sum < maxsum)
	    {
	      maxsum = sum;
	      maxindex = result[0][i].first;
	    }
	}
      if(sum < maxsum_diff)
	{
	  maxsum_diff = sum;
	}

    }

  char filename[128];
  sprintf(filename,"result/33_1.dat");
  //FILE *fp = fopen(filename,"a");
  if(maxindex != -1)
    {
      //fprintf(fp,"%f %d\n", maxsum, maxindex);
      maxSum = maxsum;
      maxIndex = maxindex;
    }
  else
    {
      //fprintf(fp,"%f %d\n", maxsum_diff, maxindex);
      maxSum = maxsum_diff;
      maxIndex = maxindex;
    }
  //fclose(fp);
}

void compareCloth::sumVecResult(vector<  vector< pair<pair<int,int>,float> > > result, float &maxSum, pair<int,int> &maxIndex)
{

  vector<int> flag;
  for(int i = 0; i < result.size(); i++)
    {
      flag.push_back(0);
    }
      //float flag[3];
  maxSum = std::numeric_limits<float>::max();
  float maxsum = std::numeric_limits<float>::max();
  float maxsum_diff = std::numeric_limits<float>::max();
  pair<int,int> maxindex(-1,-1);
  for(uint i = 0; i < result[0].size(); i++)
    {
      for(int k = 0; k< result.size() ; k++)
	flag[k] = 0;
      float sum = result[0][i].second/threVec[0];
      if(result[0][i].second < threVec[0])
	{
	  flag[0] = 1;
	}
      else
	{
	  flag[0] = 0;
	}
      for(uint k = 1; k < result.size(); k++)
	{
	  for(uint j = 0; j < result[0].size(); j++)
	    {
	      if(result[0][i].first == result[k][j].first)
		{
		  sum *= result[k][j].second/threVec[k];
		  if( result[k][j].second < threVec[k])
		    {
		      flag[k] = 1;
		    }
		  else
		    {
		      flag[k] = 0;
		    }
		}
	    }
	}

      int threFlag = 1;
      for(int j = 0; j < result.size(); j++)
	{
	  if(flag[j] == 0)
	    {
	      threFlag = 0;
	    }
	}
      if(threFlag == 1)
	{
	  if(sum < maxsum)
	    {
	      // if(sum == 0)
	      // 	{
	      // 	  for(int h = 0; h<3 ; h++)
	      // 	    {
	      // 	      cout << "hoge " << h << endl;
	      // 	      cout << " " << hoge[h] << endl;
	      // 	    }
	      // 	}
	      maxsum = sum;
	      maxindex = result[0][i].first;
	    }
	}
      if(sum < maxsum_diff)
	{
	  maxsum_diff = sum;
	  //	  if(sum == 0)
	    // {
	    //   for(int h = 0; h<3 ; h++)
	    // 	{
	    // 	  cout << "hoge " << h << " " << hoge[h] << endl;
	    // 	}
	    // }

	}

    }

  char filename[128];
  sprintf(filename,"result/33_1.dat");
  //FILE *fp = fopen(filename,"a");
  if(maxindex.first != -1)
    {
      //fprintf(fp,"%f %d\n", maxsum, maxindex);
      maxSum = maxsum;
      maxIndex = maxindex;
    }
  else
    {
      //fprintf(fp,"%f %d\n", maxsum_diff, maxindex);
      maxSum = maxsum_diff;
      maxIndex = maxindex;
    }
  //fclose(fp);
  //cout << maxSum << endl;
}

void compareCloth::sumVecResult_withStateManipulator(vector<  vector< pair<pair<int,int>,float> > > result, float &maxSum, pair<int,int> &maxIndex)
{

  vector<int> flag;
  for(uint i = 0; i < result.size(); i++)
    {
      flag.push_back(0);
    }
      //float flag[3];
  maxSum = std::numeric_limits<float>::max();
  float maxsum = std::numeric_limits<float>::max();
  float maxsum_diff = std::numeric_limits<float>::max();
  pair<int,int> maxindex(-1,-1);
  cout << "threvec " << threVec.size() << endl;
  for( uint i = 0; i < threVec.size() ; i++)
    cout << "threVec[" << i << "] " << threVec.at(i) << endl; 

  try{

    for(uint i = 0; i < result[0].size(); i++)
      {
	for(uint k = 0; k< result.size() ; k++)
	  flag[k] = 0;
	float sum = result[0][i].second/threVec.at(0);
	if(result[0][i].second < threVec.at(0))
	  {
	    flag[0] = 1;
	  }
	else
	  {
	    flag[0] = 0;
	  }

	for(uint k = 1; k < result.size(); k++)
	  {
	    for(uint j = 0; j < result[0].size(); j++)
	      {
		if(result[0][i].first == result[k][j].first)
		  {
		    sum *= result[k][j].second/threVec.at(k);
		    if( result[k][j].second < threVec.at(k))
		      {
			flag[k] = 1;
		      }
		    else
		      {
			flag[k] = 0;
		      }
		  }
	      }
	  }


	int threFlag = 1;
	for(uint j = 0; j < result.size(); j++)
	  {
	    if(flag[j] == 0)
	      {
		threFlag = 0;
	      }
	  }
	if(threFlag == 1)
	  {
	    sum /= sm.getProbability(sm.getCurrentState(),result[0][i].first.first);
	    if(sum < maxsum)
	      {
		// if(sum == 0)
		// 	{
		// 	  for(int h = 0; h<3 ; h++)
		// 	    {
		// 	      cout << "hoge " << h << endl;
		// 	      cout << " " << hoge[h] << endl;
		// 	    }
		// 	}
		maxsum = sum;
		maxindex = result[0][i].first;
	      }
	  }
	if(sum < maxsum_diff)
	  {
	    maxsum_diff = sum;
	    //	  if(sum == 0)
	    // {
	    //   for(int h = 0; h<3 ; h++)
	    // 	{
	    // 	  cout << "hoge " << h << " " << hoge[h] << endl;
	    // 	}
	    // }

	  }
      }

    char filename[128];
    sprintf(filename,"result/33_1.dat");
    //FILE *fp = fopen(filename,"a");
    if(maxindex.first != -1)
      {
	//fprintf(fp,"%f %d\n", maxsum, maxindex);
	maxSum = maxsum;
	maxIndex = maxindex;
      }
    else
      {
	//fprintf(fp,"%f %d\n", maxsum_diff, maxindex);
	maxSum = maxsum_diff;
	maxIndex = maxindex;
      }
    //fclose(fp);
    //cout << maxSum << endl;
  }
  catch (out_of_range& oor) {
    cerr << "Out of Range error: " << oor.what() << endl;
    maxSum = maxsum_diff;
    maxIndex = maxindex;

  }

}


// void compareCloth::sumVecResult(vector<  vector< pair<pair<int,int>,float> > > result, float &maxSum, pair<int,int> &maxIndex)
// {
//   int flag[3];

//   float hoge[3];
//   maxSum = std::numeric_limits<float>::max();
//   float maxsum = std::numeric_limits<float>::max();
//   float maxsum_diff = std::numeric_limits<float>::max();
//   pair<int,int> maxindex(-1,-1);
//   for(uint i = 0; i < result[0].size(); i++)
//     {
//       for(int k = 0; k<3 ; k++)
// 	flag[k] = 0;
//       float sum = result[0][i].second/sprhThre;
//       hoge[0] = result[0][i].second;
//       if(result[0][i].second < sprhThre)
// 	{
// 	  flag[0] = 1;
// 	}
//       else
// 	{
// 	  flag[0] = 0;
// 	}

//       for(uint j = 0; j < result[0].size(); j++)
// 	{
// 	  if(result[0][i].first.first == result[1][j].first.first)
// 	    {
// 	      sum *= result[1][j].second/histThre;
// 	      hoge[1] = result[1][i].second;
// 	      if( result[1][j].second < histThre)
// 		{
// 		  flag[1] = 1;
// 		}
// 	      else
// 		{
// 		  flag[1] = 0;
// 		}
// 	    }
// 	  if(result[0][i].first.first == result[2][j].first.first)
// 	    {
// 	      sum *= result[2][j].second/partThre;
// 	      hoge[2] = result[2][i].second;
// 	      if(result[2][j].second < partThre)
// 		{sum
// 		  flag[2] = 1;
// 		}
// 	      else
// 		{
// 		  flag[2] = 0;
// 		}
// 	    }
// 	}
//       if(flag[0] == 1 && flag[1] == 1 && flag[2] == 1)
// 	{
// 	  if(sum < maxsum)
// 	    {
// 	      cout << "maxsum use" << endl;
// 	      //	      if(sum == 0)
// 		{
// 		  for(int h = 0; h<3 ; h++)
// 		    {
// 		      cout << "hoge " << h << endl;
// 		      cout << " " << hoge[h] << endl;
// 		    }
// 		}
// 	      maxsum = sum;
// 	      maxindex = result[0][i].first;
// 	    }
// 	}
//       if(sum < maxsum_diff)
// 	{
// 	  maxsum_diff = sum;
// 	  //	  if(sum == 0)
// 	    {
// 	      for(int h = 0; h<3 ; h++)
// 		{
// 		  cout << "hoge " << h << " " << hoge[h] << endl;
// 		}
// 	    }

// 	}

//     }

//   char filename[128];
//   sprintf(filename,"result/33_1.dat");
//   //FILE *fp = fopen(filename,"a");
//   if(maxindex.first != -1)
//     {
//       //fprintf(fp,"%f %d\n", maxsum, maxindex);
//       maxSum = maxsum;
//       maxIndex = maxindex;
//     }
//   else
//     {
//       //fprintf(fp,"%f %d\n", maxsum_diff, maxindex);
//       maxSum = maxsum_diff;
//       maxIndex = maxindex;
//     }
//   //fclose(fp);
//   cout << maxSum << endl;
// }

int compareCloth::getCheckStateResult()
{
  return checkStateResult;
}

int compareCloth::checkState(int matchState)
{
  if(matchState != -1)
    {
      //cout << dirname << " " <<  matchState << endl;
      indexlist.pop_front();
      indexlist.push_back(matchState);
      int flag = 1;
      int indextemp = 0;  
    for(std::list<int>::iterator i = indexlist.begin(); i != indexlist.end(); i++)
	{
	  if(indextemp == 0)
	    {
	      indextemp = (*i);
	    }
	  if(indextemp != (*i))
	    {
	      flag = 0;
	    }
	}

      if(flag == 1)
	{
	  if(sm.stateChenge(indextemp))
	    {
	      checkStateResult = indextemp;
	      return indextemp;
	    }
	  else
	    {
	      checkStateResult = -1;
	      return -1;
	    }
	}
      else
	{
	  checkStateResult = -1;
	  return -1;
	}
    }
  checkStateResult = -1;
  return -1;
}

void compareCloth::convertOldFile2NewFile()
{
  DIR *dir_ptr;
  struct dirent *ent;
  char flowdir[128];
  sprintf(flowdir,"%s/all_flow/", dirname);
  if((dir_ptr = opendir(flowdir)) == (DIR *)NULL)
    {
      std::cerr << "no all_flow directory" << std::endl;
      exit(1);
    }



  //指定したディレクトリ内部には特徴ファイル以外は存在しないことを想定
  FILE *fp;


  int featcount=0,imnum=0;
  int stateNum, imageNum;
  for(int i=0; (ent = readdir(dir_ptr)) != (struct dirent *)NULL; i++)
    {
      if(ent->d_name[0] != '.' && sscanf(ent->d_name,"allflow_%05d.dat",&imnum) != 0)
	{
	  vector<Point2f> flowPoint,flowScale;
	  cv::Mat src_img;
	  float x,y,len,ang;

	  if(featcount == 0)
	    {


	      string opf = string(flowdir) + string(ent->d_name);
	      fp = fopen(opf.c_str(),"r");
	      while(fscanf(fp,"%f %f %f %f\n",&x, &y, &len, &ang) != EOF)
		featcount++;
	      fclose(fp);
	    }


	  vector<float> vecx,vecy,veclen,vecang;
	  char file[128];
	  sprintf(file, "%s/dst_img/flowimage_%05d.ppm", dirname, imnum);

	  string opf = string(dirname) + string("/all_flow/") + string(ent->d_name);
	  if((fp = fopen(opf.c_str(),"r")) == NULL)
	    {
	      cerr << "no flowimage file" << endl;
	      printf("%s\n",opf.c_str());
	      exit(1);
	    }


	  while(fscanf(fp,"%f %f %f %f\n",&x, &y, &len, &ang) != EOF)
	    {

	      vecx.push_back(x);
	      vecy.push_back(y);
	      veclen.push_back(len);
	      vecang.push_back(ang);

	    }



	  fclose(fp);


	  stateNum = calcStateNum(imnum);
	  imageNum = calcImageNum(imnum);
	  src_img = imread( file);
	  vector< pair<P2d,P2d> > vals;
	  for(uint k = 0; k < vecx.size(); k++)
	    {

	      vals.push_back( pair<P2d,P2d>(P2d(vecx[k],vecy[k]),P2d(veclen[k],vecang[k])));
	    }

	  convertP2d2flowPoint(vals, &flowPoint, &flowScale);
	  eachImageNum = imageNum;
	  currentState = stateNum;
	  genVecFiles(flowPoint, flowScale, src_img);

	}
    }
}


int compareCloth::showDatabaseFlow(int stateNum, int imageNum)
{
  int imnum;
  char imageFile[128];
  sprintf(imageFile, "%s/src_image/src_image_%04d_%07d.ppm",dirname, stateNum, imageNum);
  Mat src_img = imread(imageFile);


  DIR *dir_ptr;
  struct dirent *ent;
  char flowdir[128];
  sprintf(flowdir,"%s/all_flow/", dirname);
  if((dir_ptr = opendir(flowdir)) == (DIR *)NULL)
    {
      std::cerr << "no all_flow directory" << std::endl;
      exit(1);
    }

  //指定したディレクトリ内部には特徴ファイル以外は存在しないことを想定
  FILE *fp;

  vector< pair<P2d,P2d> > vals;
  int flowNum = 1000 * stateNum + imageNum;

  for(int i=0; (ent = readdir(dir_ptr)) != (struct dirent *)NULL; i++){
    if(ent->d_name[0] != '.' && sscanf(ent->d_name,"allflow_%05d.dat",&imnum) != 0)
      {
        float x;
        float y;
        float len;
        float ang;
	string opf = string(flowdir) + string(ent->d_name);

	if(imnum ==  flowNum)
	  {

	    if((fp = fopen(opf.c_str(),"r")) == NULL)
	      {
		cerr << "no database file" << endl;
		printf("%s\n",opf.c_str());
		exit(1);
	      }
	    while(fscanf(fp,"%f %f %f %f\n",&x, &y, &len, &ang) != EOF)
	      {

		pair<P2d,P2d> oneFlow = pair<P2d,P2d>(P2d(x,y),P2d(len,ang));
		vals.push_back(oneFlow);
	      }
	    fclose(fp);
	  }

      }
  }

  Mat flow_image;
  display(16, src_img, flow_image, vals, "databaseFlow");

  return 0;
}
